package com.dji.ux.sample.Utils;

public class PIDUtil {

    //PID参数
    private float KP = 0f;
    private float KI = 0f;
    private float KD = 0f;

    private float abs_error = 0.0f; //当前帧像素距离绝对值
    private float abs_errorLast = 0.0f; //前一帧像素距离绝对值

    private float error = 0.0f; //当前帧像素距离
    private float errorLast = 0.0f; //前一帧像素距离

    private float integral = 0.0f;  //PID积分项

    public PIDUtil() {
    }

    public float getKP() {
        return KP;
    }

    public void setKP(float KP) {
        this.KP = KP;
    }

    public float getKI() {
        return KI;
    }

    public void setKI(float KI) {
        this.KI = KI;
    }

    public float getKD() {
        return KD;
    }

    public void setKD(float KD) {
        this.KD = KD;
    }

    public float getIntegral() {
        return integral;
    }

    public float getError() {
        return error;
    }

    /**
     * 初始化PID参数
     */
    public void reset() {
        this.abs_error = 0.0f;
        this.abs_errorLast = 0.0f;
        this.error = 0.0f;
        this.errorLast = 0.0f;
        this.integral = 0.0f;
    }

    /**
     *
     * @param error 二次定位算法输出的像素距离（dx,dy）
     * @param curr_height 无人机当前高度
     * @return
     */
    public float PID_Cal(float error, float curr_height){
        float speed = 0.0f;
        this.abs_error = Math.abs(error);   //像素距离绝对值
        this.error = error; //像素距离
        this.integral += error; //每一帧的像素距离累加，得出PID积分项

        /*PID速度计算公式*/
        speed = this.KP * curr_height * this.abs_error - this.KI * curr_height * this.integral + this.KD * curr_height * (this.abs_error - this.abs_errorLast);

        if (curr_height > 5f) speed *= 0.5; //高度大于5米，速度*0.5

        this.abs_errorLast = this.abs_error;    //保存当前帧像素距离的绝对值，为下一帧计算做准备
        this.errorLast = this.error;    //保存当前帧像素距离，为下一帧计算做准备

        /*若前一帧和当前帧的像素距离方向相反，则积分项归零*/
        if (this.error * this.errorLast < 0){
            this.integral = 0;
        }
        /*返回与像素距离同号的速度值*/
        return error > 0 ? speed : -speed;
    }
}
